#include "Sensors.h"

#include "FastSerial.h"
#include <Arduino.h>

#define radtodeg(x) (x*57.2957795)
#define degtorad(x) (x*0.0174532925)

void Sensors::init(AP_PeriodicProcess *scheduler, AP_IMU_INS &myimu, AP_DCM &mydcm, MyGPS &gps)
{
	dcm = &mydcm;
	imu = &myimu;
	this->gps = &gps;
	
	vrate = 0;
	lastVrateTime = 0;
	
	imu->init(IMU::COLD_START, delay, flash_leds, scheduler);
	imu->init_accel(delay, flash_leds);
	dcm->set_centripetal(1);
	dcm->matrix_reset();
	this->gps->init();
	double ax,ay,az;
	ax = -this->ax();
	ay = this->ay();
	az = -this->az();
	Serial.println();
	Serial.print("AX = ");
	Serial.print(ax);
	Serial.print("   AY = ");
	Serial.print(ay);
	Serial.print("   AZ = ");
	Serial.println(az);
	calibX = atan2(ax,az)*57.2957795;
	calibY = atan2(ay,az)*57.2957795;
	Serial.print("Cx = ");
	Serial.print(calibX);
	Serial.print("\tCy = ");
	Serial.println(calibY);
	Serial.println("DONE WITH SETUP!");
}

void Sensors::flash_leds(bool on)
{
  digitalWrite(35, !on);
  digitalWrite(37, on);
  digitalWrite(36, HIGH);
}

void Sensors::update()
{
	gps->update();
	dcm->update_DCM();
	battery = analogRead(0);
}
double Sensors::getLat()
{
	return gps->status()==GPS::GPS_OK?gps->my_lat:0.0;
}
double Sensors::getLon()
{
	return gps->status()==GPS::GPS_OK?gps->my_lon:0.0;
}
double Sensors::getRoll(){return radtodeg(dcm->roll)+calibX;}
double Sensors::getPitch(){return radtodeg(dcm->pitch)+calibY;}
double Sensors::getYaw(){return radtodeg(dcm->yaw);}
double Sensors::getCourse(){return gps->my_course;}
double Sensors::getHeading(){return radtodeg(dcm->yaw);}
double Sensors::getVrate()
{
	double newAlt = getAlt();
	int dt = millis()-lastVrateTime;
	if (dt > 1000)
	{
		//over a second, bad readings
		lastAlt = newAlt;
		vrate = 0;
	}
	else if (dt == 0)
	{
		vrate = newAlt-lastAlt/(dt*1000.0);
	}
	lastAlt = newAlt;
	return vrate;
}
double Sensors::getSpeed(){return gps->my_speed;}
double Sensors::getAlt()
{
	return gps->status()==GPS::GPS_OK?gps->my_alt:0.0;
}
double Sensors::getSlip(){return ax();}
double Sensors::getG(){return sqrt((ax()*ax()) + (ay()*ay()) + (az()*az()));}
